{
 "cells": [
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Script to get the jacobian matrix which under the .ipynb format"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 19,
   "metadata": {},
   "outputs": [],
   "source": [
    "import sympy as sym\n",
    "import numpy as np"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 20,
   "metadata": {},
   "outputs": [],
   "source": [
    "x, y, z, w, l, h, vx, vy, vz, ax, ay, az, yaw = sym.symbols(\"x y z w l h vx vy vz ax ay az yaw\")\n",
    "next_x, next_y, next_z, next_w, next_l, next_h, next_vx, next_vy, next_vz, next_ax, next_ay, next_az, next_yaw = sym.symbols(\"next_x next_y next_z next_w next_l next_h next_vx next_vy next_vz next_ax next_ay next_az next_yaw\")\n",
    "dt = sym.symbols(\"dt\")"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 21,
   "metadata": {},
   "outputs": [],
   "source": [
    "next_w, next_l, next_h, next_yaw = w, l, h, yaw\n",
    "next_ax, next_ay, next_az = ax, ay, 0\n",
    "next_vx = vx + ax * dt\n",
    "next_vy = vy + ay * dt\n",
    "next_vz = 0\n",
    "next_x = x + vx * dt + 1 / 2 * ax * dt * dt\n",
    "next_y = y + vy * dt + 1 / 2 * ay * dt * dt\n",
    "next_z = z"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 22,
   "metadata": {},
   "outputs": [],
   "source": [
    "funcs = sym.Matrix([next_x, next_y, next_z, next_w, next_l, next_h, next_vx, next_vy, next_vz, next_ax, next_ay, next_az, next_yaw])\n",
    "args = sym.Matrix([x, y, z, w, l, h, vx, vy, vz, ax, ay, az, yaw])\n",
    "res = funcs.jacobian(args)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 23,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\left[\\begin{array}{ccccccccccccc}1 & 0 & 0 & 0 & 0 & 0 & dt & 0 & 0 & 0.5 dt^{2} & 0 & 0 & 0\\\\0 & 1 & 0 & 0 & 0 & 0 & 0 & dt & 0 & 0 & 0.5 dt^{2} & 0 & 0\\\\0 & 0 & 1 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\\\0 & 0 & 0 & 1 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\\\0 & 0 & 0 & 0 & 1 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\\\0 & 0 & 0 & 0 & 0 & 1 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\\\0 & 0 & 0 & 0 & 0 & 0 & 1 & 0 & 0 & dt & 0 & 0 & 0\\\\0 & 0 & 0 & 0 & 0 & 0 & 0 & 1 & 0 & 0 & dt & 0 & 0\\\\0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\\\0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 1 & 0 & 0 & 0\\\\0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 1 & 0 & 0\\\\0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\\\0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 1\\end{array}\\right]$"
      ],
      "text/plain": [
       "Matrix([\n",
       "[1, 0, 0, 0, 0, 0, dt,  0, 0, 0.5*dt**2,         0, 0, 0],\n",
       "[0, 1, 0, 0, 0, 0,  0, dt, 0,         0, 0.5*dt**2, 0, 0],\n",
       "[0, 0, 1, 0, 0, 0,  0,  0, 0,         0,         0, 0, 0],\n",
       "[0, 0, 0, 1, 0, 0,  0,  0, 0,         0,         0, 0, 0],\n",
       "[0, 0, 0, 0, 1, 0,  0,  0, 0,         0,         0, 0, 0],\n",
       "[0, 0, 0, 0, 0, 1,  0,  0, 0,         0,         0, 0, 0],\n",
       "[0, 0, 0, 0, 0, 0,  1,  0, 0,        dt,         0, 0, 0],\n",
       "[0, 0, 0, 0, 0, 0,  0,  1, 0,         0,        dt, 0, 0],\n",
       "[0, 0, 0, 0, 0, 0,  0,  0, 0,         0,         0, 0, 0],\n",
       "[0, 0, 0, 0, 0, 0,  0,  0, 0,         1,         0, 0, 0],\n",
       "[0, 0, 0, 0, 0, 0,  0,  0, 0,         0,         1, 0, 0],\n",
       "[0, 0, 0, 0, 0, 0,  0,  0, 0,         0,         0, 0, 0],\n",
       "[0, 0, 0, 0, 0, 0,  0,  0, 0,         0,         0, 0, 1]])"
      ]
     },
     "execution_count": 23,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "res"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 24,
   "metadata": {},
   "outputs": [],
   "source": [
    "# measure infos have velocity\n",
    "with_velo_mea_funcs = sym.Matrix([x, y, z, w, l, h, vx, vy, yaw])\n",
    "with_velo_mea_res = with_velo_mea_funcs.jacobian(args)\n",
    "\n",
    "# measure infos haven't velocity info\n",
    "no_velo_mea_funcs = sym.Matrix([x, y, z, w, l, h, yaw])\n",
    "no_velo_mea_res = no_velo_mea_funcs.jacobian(args)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 25,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "(Matrix([\n",
       " [1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],\n",
       " [0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],\n",
       " [0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],\n",
       " [0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0],\n",
       " [0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0],\n",
       " [0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0],\n",
       " [0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0],\n",
       " [0, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0],\n",
       " [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1]]),\n",
       " Matrix([\n",
       " [1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],\n",
       " [0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],\n",
       " [0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],\n",
       " [0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0],\n",
       " [0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0],\n",
       " [0, 0, 0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0],\n",
       " [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 1]]))"
      ]
     },
     "execution_count": 25,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "with_velo_mea_res, no_velo_mea_res"
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3 (ipykernel)",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.7.16"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}
